#include <iostream>
#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <opencv2/opencv.hpp>

#include <thread>
#include <termios.h>
#include "utm_transformer.h"
// system include.
#include <pthread.h>
#include <thread>
#include <iostream>
#include <fstream>
#include <string.h>
// apply include.
#include <mutex>
#include <cstdlib> 
#include "control.pb.h"
#include "localization.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>

#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>

typedef struct
{
    double lat_degree;
    double lon_degree;
} COORDINATE; // coordinate

namespace bg = boost::geometry;
using point_t = bg::model::d2::point_xy<double>;
using polygon_t = bg::model::polygon<point_t>; 
using mpolygon_t = bg::model::multi_polygon<polygon_t>;



class C_bulidMapApi
{
private:
    /* data */
    double car_width ;
    double car_height;
    double meter_to_centi; // 米到厘
    double coordinate_offset_x;
    double coordinate_offset_y;
    double tolerance;
    double map_width;
    double map_height;
    double dis_threshold; // 距离阈值
    bool polygon_feasible;
    double lat_degree;
    double lon_degree;
    std::vector<std::vector<double>> car_coordinate; // 储存坐标信息的二维数组，n行2列，储存n个横纵坐标

    std::vector<double> max_abs_negative; // 保存图像的最大宽度
    std::vector<double> max_abs_positive; 

    double first_point_x; // 使用numeric_limits方法将定义的变量设置极值
    double first_point_y;

    /* function */
    double ConvertToDegrees(const std::string& in_data1, const std::string& in_data2); //把读取到的经纬度数据转换成标准格式，例如 310.11231 -> 31.011231

    double ConvertToDegrees_double(double& in_data); // 把读取到的经纬度数据转换成标准格式，例如 310.11231 -> 31.011231

    void draw_arrow(cv::Point start_point, cv::Point end_point, cv::Mat& image); // 绘制箭头的线段 代表朝向，仅仅用于展示建图过程

    void draw_rotated_rect(cv::Point point1, cv::Point point2, double car_width, double car_height, cv::Mat& image) ; // 绘制旋转矩形,用于代表小车，实时显示小车动态以及最后展示

    mpolygon_t expand_polygon(const polygon_t& polygon, const double distance); // 多边形膨胀，用于对地图整体尺寸进行缩放，绘制地图安全区域边界

    double calculateDistance(const std::vector<double>& point1, const std::vector<double>& point2); // 计算两点之间的距离

    std::vector<std::vector<double>> removeDuplicateRows_read_data(const std::vector<std::vector<double>>& input); // 删除重复行读取数据，对每一行的数据进行判断去重

    static void fifoSaveDate(double lat_degree, double lon_degree); // 通过一个vector buf来转存回调函数中接收到的数据，形成一个fifo结构。防止数据丢失处理不过来 // 回调函数内都需呀使用静态函数和静态变量

    bool getFifoDate(COORDINATE &tempCoordinate); // 获取fifo中转存的数据内容，按照顺序输出；

    void keyboardThread() ; // 线程函数，涉及按键触发建图线程启停

    void DisplayBulidMap(std::vector<std::vector<double>> car_coordinate_copy); // 使用opencv的图片刷新显示建图过程

    void saveImage(cv::Mat &image, std::vector<cv::Point> &polygon_points_small, polygon_t &polygon_after_simplified);

    void bulidMap(double lat_degree, double lon_degree, eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage); // 创建地图算法，单独一个线程中调用

public:

    C_bulidMapApi(/* args */);
    ~C_bulidMapApi();

    void initBulidMap(void);
    /***********************************************
     * @author: Guowanda
     * @date:   23/10
     * @brief:  回调函数，通过ecal订阅获取坐标的报文进行回调
     * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
     * @return: void
     ***********************************************/
    static void Callback(const MowerProtoPakg::localizationGNGGA& msg) ; // 回调函数内都需呀使用静态函数和静态变量
};


